#include "Vector3.h"

void minus3_3(double l[3], double r[3], double ans[3])
{
    // 两向量相减
    for(int i = 0; i < 3; i++)
    {
        ans[i] = l[i] - r[i];
    }
}

void dot33_33(const double left33[3][3], const double right33[3][3], double ans33[3][3])
{
    // 3*3 矩阵 * 3*3矩阵
    for(int i = 0; i < 3; i++)
    {
        for(int j = 0; j < 3; j++)
        {
            ans33[i][j] = left33[i][0] * right33[0][j] 
                        + left33[i][1] * right33[1][j] 
                        + left33[i][2] * right33[2][j];
        }
    }
}

void dot33_3(const double left33[3][3], const double right3[3], double ans3[3])
{
    // 3*3矩阵 * 3*1 矩阵
    for(int i = 0; i < 3; i++)
    {
        ans3[i] = left33[i][0] * right3[0]
			    + left33[i][1] * right3[1]
			    + left33[i][2] * right3[2];
    }
}

void dotk3(const double k, const double vector3[3], double ans3[3])
{
    // k 增益
    for(int i = 0; i < 3; i++)
    {
        ans3[i] = k*vector3[i];
    }
}

double dot3_3(const double left3[3], const double right3[3])
{
    // 两向量对应位置元素分别相乘再求和
    double ans = left3[0] * right3[0] 
               + left3[1] * right3[1] 
               + left3[2] * right3[2];
    return ans;
}

double dot3_3(const double vector3[3])
{
    // 向量各分量的平方和
    double ans = vector3[0] * vector3[0]+
                 vector3[1] * vector3[1]+
                 vector3[2] * vector3[2];
    return ans;
}

double norm3(const double vector3[3])
{
    // 向量的范数
    return sqrt(dot3_3(vector3));
}

void uni3(const double vector3[3], double ans3[3])
{
    // 向量单位化
    double vector_norm = norm3(vector3);
    for(int i = 0; i < 3; i++)
    {
        ans3[i] = vector3[i] / vector_norm;
    }
}

void cross3_3(const double left3[3], const double right3[3], double ans3[3])
{
    // 向量叉乘
    ans3[0] = left3[1] * right3[2] - left3[2] * right3[1];
	ans3[1] = left3[2] * right3[0] - left3[0] * right3[2];
	ans3[2] = left3[0] * right3[1] - left3[1] * right3[0];
}

void transpose33(const double m33[3][3], double tm33[3][3])
{
    // 矩阵转置
    for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			tm33[i][j] = m33[j][i];
		}
	}
}

